﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;

namespace TrackingSim.GameSystem
{
    class Coords
    {
        public static MathLib.Matrix abs_to_nav()
        {
            MathLib.Matrix nav_to_abs_ret = new MathLib.Matrix(3, 3);

            Matrix nav_to_abs = Matrix.CreateWorld(new Vector3(0.0f, 0.0f, 0.0f),  //Position
                                                   new Vector3(1.0f, 0.0f, 0.0f),  //Forward
                                                   new Vector3(0.0f, 0.0f, 1.0f)); //Up

            nav_to_abs_ret.m[0, 0] = nav_to_abs.M11; nav_to_abs_ret.m[0, 1] = nav_to_abs.M12; nav_to_abs_ret.m[0, 2] = nav_to_abs.M13;
            nav_to_abs_ret.m[1, 0] = nav_to_abs.M21; nav_to_abs_ret.m[1, 1] = nav_to_abs.M22; nav_to_abs_ret.m[1, 2] = nav_to_abs.M23;
            nav_to_abs_ret.m[2, 0] = nav_to_abs.M31; nav_to_abs_ret.m[2, 1] = nav_to_abs.M32; nav_to_abs_ret.m[2, 2] = nav_to_abs.M33;

            return nav_to_abs_ret;
        }

        public static MathLib.Matrix abs_to_body(Player p)
        {
            MathLib.Matrix world = get_rotation_matrix_from_xna_matrix(p.WorldMatrix);
            MathLib.Matrix nav = abs_to_nav().tp();
            MathLib.Matrix body = nav * world;

            return body;
        }

        public static MathLib.Matrix abs_to_body_6x6(Player p)
        {
            MathLib.Matrix m = new MathLib.Matrix(6, 6);
            MathLib.Matrix a2b = abs_to_body(p);

            for (int i = 0; i < 3; ++i)
            {
                for (int j = 0; j < 3; ++j)
                {
                    m.m[i, j] = a2b.m[i, j];
                    m.m[i+3, j+3] = a2b.m[i, j];
                }
            }

            return m;
        }

        public static MathLib.Matrix body_to_nav(Player p)
        {
            return abs_to_nav() * abs_to_body(p);
        }

        public static MathLib.Matrix get_rotation_matrix_from_xna_matrix(Matrix m)
        {
            MathLib.Matrix mat = new MathLib.Matrix(3, 3);

            mat.m[0, 0] = m.M11; mat.m[0, 1] = m.M12; mat.m[0, 2] = m.M13;
            mat.m[1, 0] = m.M21; mat.m[1, 1] = m.M22; mat.m[1, 2] = m.M23;
            mat.m[2, 0] = m.M31; mat.m[2, 1] = m.M32; mat.m[2, 2] = m.M33;

            return mat;
        }

        public static Matrix get_xna_matrix_from_rotation_matrix(MathLib.Matrix mat)
        {
            Matrix m = new Matrix();
            m.Translation = Vector3.Zero;

            m.M11 = (float)mat.m[0, 0]; m.M12 = (float)mat.m[0, 1]; m.M13 = (float)mat.m[0, 2];
            m.M21 = (float)mat.m[1, 0]; m.M22 = (float)mat.m[1, 1]; m.M23 = (float)mat.m[1, 2];
            m.M31 = (float)mat.m[2, 0]; m.M32 = (float)mat.m[2, 1]; m.M33 = (float)mat.m[2, 2];

            return m;
        }

        public static MathLib.Vector get_vector_from_xna_vector(Vector3 v)
        {
            MathLib.Vector vec = new MathLib.Vector(3);
            
            vec.v[0] = v.X;
            vec.v[1] = v.Y;
            vec.v[2] = v.Z;

            return vec;
        }

        public static Vector3 get_xna_vector_from_vector(MathLib.Vector v)
        {
            Vector3 vec = new Vector3((float)v.v[0], (float)v.v[1], (float)v.v[2]);
            return vec;
        }
    }
}
